{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "connect_robot = True\n",
    "import almath\n",
    "from time import sleep\n",
    "from nao import Nao\n",
    "from robot import rotx,roty,rotz"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "nao = Nao(\"127.0.0.1\",9559)\n",
    "nao.simulationInitPose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 单机器人两个机械臂的协同\n",
    "\n",
    "设在一个坐标系下,两个臂的末端矩阵表示分别为: ${}_L^B T, {}_R^B T$\n",
    "\n",
    "所以: $ {}_R^L T = {}_B^L T {}_R^B T$\n",
    "\n",
    "变化后已知 ${}_{L'}^B T, 求 {}_{R'}^B T$ 因为两者间相互位姿不变,\n",
    "\n",
    "故: $ {}_{R'}^{L'} T = {}_R^L T = {}_B^L T {}_R^B T$\n",
    "\n",
    "且: $ {}_{R'}^B T =  {}_{L'}^B T {}_{R'}^{L'} T = {}_{L'}^B T {}_B^L T {}_R^B T  = \n",
    "{}_{L'}^B T ({}_L^B T)^{-1} {}_R^B T $"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def cooperation(main,slave,new_main):\n",
    "    new_slave = new_main*inv(main)*slave\n",
    "    return new_slave"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "LTf = nao.getLArmTf()\n",
    "RTf = nao.getRArmTf()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nLTf = LTf.copy()\n",
    "nLTf[1,3] += 10 # y 方向平移 10 mm\n",
    "nLTf[2,3] += 5 # z 方向平移 5 mm\n",
    "nRTf = cooperation(LTf,RTf,nLTf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for x in range(3):\n",
    "    nao.setLArmTf2(nLTf)\n",
    "    nao.setRArmTf2(nRTf)\n",
    "    sleep(1)\n",
    "    nao.setLArmTf2(LTf)\n",
    "    nao.setRArmTf2(RTf)\n",
    "    sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "nLTf = LTf.copy()\n",
    "nLTf[:3,:3] = nLTf[:3,:3] * rotx(0.01)\n",
    "nRTf = cooperation(LTf,RTf,nLTf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for x in range(10):\n",
    "    nao.setLArmTf2(nLTf)\n",
    "    nao.setRArmTf2(nRTf)\n",
    "    sleep(1)\n",
    "    nao.setLArmTf2(LTf)\n",
    "    nao.setRArmTf2(RTf)\n",
    "    sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
